import math

from plan_tool.Node import Node
from plan_tool.point import point


def getdis(point1, point2):  # 曼哈顿距离
    dis = math.sqrt(pow(point1.x - point2.x, 2) + pow(point1.y - point2.y, 2))
    return dis


class Astar:  ##定义A*算法中的函数
    def __init__(self, maze, startnode, endnode, length, width, Global_node_list):
        self.openList = []
        self.closeList = []  ##维护的open与close表
        self.maze = maze  ##地图信息，记录不可行点
        self.startNode = startnode
        self.endNode = endnode
        self.currNode = startnode  ##全局起始终点节点信息，以及更新的当前节点
        self.path = []  ##记录路径
        self.length = length
        self.width = width
        self.node_list = Global_node_list

    def minFvalue(self):  ##计算openlist中的最小f节点
        if len(self.openList) == 0:
            return None
        minNode = self.openList[0]
        for tempNode in self.openList:
            if tempNode.f < minNode.f:
                minNode = tempNode
        return minNode

    def isOpen(self, thisNode):  ##判断该节点是否在openlist中
        for tempNode in self.openList:
            if tempNode.point.x == thisNode.point.x and tempNode.point.y == thisNode.point.y:
                return True
        return False

    def isclose(self, thisNode):  ##判断该节点是否在closelist中
        for tempNode in self.closeList:
            if tempNode.point.x == thisNode.point.x and tempNode.point.y == thisNode.point.y:
                return True
        return False

    def isLaststep(self):  ##判断openlist中是否含有end点，判断是否为最后一步
        for tempNode in self.openList:
            if tempNode.point.x == self.endNode.point.x and tempNode.point.y == self.endNode.point.y:
                return True
        return False

    def getNodeinOL(self, thisNode):  ##通过坐标索引open表中的节点和信息
        for tempNode in self.openList:
            if thisNode.point.x == tempNode.point.x and thisNode.point.y == tempNode.point.y:
                return tempNode
        return None

    def isAccess(self, thisPoint):  ##判断point是否可通行
        # print(thisPoint.x,thisPoint.y)
        if thisPoint.x < 0 or thisPoint.y < 0:  ##低于下界
            return False
        if thisPoint.x >= self.length - 1 or thisPoint.y >= self.width - 1:  ##高于上界
            return False
        for obPoint in self.maze:  ##处于障碍物点
            if pow(obPoint[0] - thisPoint.x, 2) + pow(obPoint[1] - thisPoint.y, 2) <= pow(obPoint[2], 2) + 1:
                return False
        return True

    def searchNode(self, thisNode):
        gValue = getdis(thisNode.point, self.currNode.point)
        # print("进行search")
        if not self.isAccess(thisNode.point):  ##不可行点
            # print("不可行")
            return
        if self.isclose(thisNode):  ##已经遍历过该点
            # print("遍历过")
            return
        if not self.isOpen(thisNode):  ##不在open表中，则进行操作
            # print("不在open表")
            thisNode.getGvalue(gValue + self.currNode.g)  ##更新该节点g
            thisNode.getManhattendis(self.endNode)  ##更新该节点h
            thisNode.ancient = self.currNode  ##更新该节点父亲节点
            self.openList.append(thisNode)  ##加入open表中
        else:  ##若已经在open表中
            # print("在open表")
            tempNode = self.getNodeinOL(thisNode)  ##把open表中的该点找出来
            if self.currNode.g + gValue < tempNode.g:  ##更新g、以及父亲节点
                tempNode.g = self.currNode.g + gValue
                tempNode.ancient = self.currNode
        return

    def searchWay(self):  ##定义搜索
        # print(len(self.currNode.neighbor))
        for i in range(len(self.currNode.neighbor)):
            if self.isAccess(self.currNode.neighbor[i].point):
                # print(self.currNode.neighbor[i].point.x, self.currNode.neighbor[i].point.y)
                self.searchNode(self.currNode.neighbor[i])
        return

    def run(self):
        # print("endnode",self.endNode.point.x, self.endNode.point.y)
        # print("startnode", self.startNode.point.x, self.startNode.point.y)
        self.startNode.getManhattendis(self.endNode)
        self.startNode.getGvalue(0)  ##初始化起点的g与h以及f

        self.openList.append(self.startNode)
        self.currNode = self.minFvalue()  ##将初始点加入open表中，并作为currNode
        self.closeList.append(self.currNode)  ##将该节点放入close表中
        self.searchWay()  ##探索周围节点
        self.openList.remove(self.currNode)  ##探索完毕，从open中删除该节点
        while True:  ##开始遍历
            # print(self.currNode.point.x, self.currNode.point.y)
            # print(self.openList)
            tt_node = self.currNode
            self.currNode = self.minFvalue()  ##从open表中选currNode进行操作

            if self.currNode is None:
                tempNode = tt_node
                self.path.append(self.endNode)
                while True:
                    self.path.append(tempNode)
                    if tempNode.ancient is not None:
                        tempNode = tempNode.ancient
                    else:
                        return False

            self.closeList.append(self.currNode)
            self.searchWay()
            self.openList.remove(self.currNode)

            if self.isLaststep():  ##下一步可以到达终点(在open表中说明已经建立父节点关系)
                tempNode = self.getNodeinOL(self.endNode)
                while True:
                    self.path.append(tempNode)
                    if tempNode.ancient is not None:
                        tempNode = tempNode.ancient
                    else:
                        print(len(self.path), len(self.path))
                        if len(self.path) == 2:
                            print(self.path)
                        return self.path
            elif len(self.openList) == 0:
                return False
